









// THIS FILE IS OBSOLETE










///*
// * To change this template, choose Tools | Templates
// * and open the template in the editor.
// */
//package edu.wpi.first.wpilibj.templates.commands;
//
//import edu.wpi.first.wpilibj.templates.InertialReference;
//import edu.wpi.first.wpilibj.templates.subsystems.LifterSubsystem;
//
///**
// *
// * @author matt
// */
//public class Lifter extends CommandBase {
//
//    // Control constants
//    final double kP = .5;
//    final double kI = 0;
//    final double positionTolerance = 1;
//    // Operating variables
//    double lastPositionError;
//    double headingError;
//    double accumulatedPositionError;
//    double newHeading;
//    double commandedExtention;
//    boolean moveComplete = false;
//    double positionError;
//    // needed classes
//
//    
//    
//    public Lifter(double extention) {
//        // Use requires() here to declare subsystem dependencies
//        requires(rightRearLifterSubsystem);
//        
//
//        commandedExtention = extention;
//
//        System.out.println("Lifter Constructed");
//    }
//
//    // Called just before this Command runs the first time
//    protected void initialize() {
//
//        // initialize the control loop variables
//        lastPositionError = 0;
//        headingError = 0;
//        accumulatedPositionError = 0;
//        positionError = 0;
//
//        moveComplete = false;
//    }
//
//
//
//    // Called repeatedly when this Command is scheduled to run
//    protected void execute() {
//        // We'll do our own PI here
//        double move = 0;
//        System.out.print("Lifting... ");
//        //
//        // figure out how far off on the heading we are
//        //headingError = computeHeadingChange(newHeading);
//        
//        //positionError = commandedExtention - position;
//
//        System.out.print(" e" + positionError);
//        // reset error accumulator if we overshoot
//        if (0 > positionError * lastPositionError) {
//            accumulatedPositionError = 0;
//        }
//
//        // figure out if we're done
//        if ((-positionTolerance < positionError)
//                && (positionTolerance > positionError)) {
//            moveComplete = true;
//        } else {
//            move = (kP * positionError) + (kI * accumulatedPositionError);
//            // move = move/180;
//            System.out.print(" s" + move);
//
//        }
//        // set up for next loop
//        lastPositionError = positionError;
//        System.out.println("-");
//        rightRearLifterSubsystem.setLifterPosition(0);
//    }
//
//    // Make this return true when this Command no longer needs to run execute()
//    protected boolean isFinished() {
//        return moveComplete;
//    }
//
//    // Called once after isFinished returns true
//    protected void end() {
//
//    }
//
//    // Called when another command which requires one or more of the same
//    // subsystems is scheduled to run
//    protected void interrupted() {
//        // set the drive mode back to what is expected if we're interrupted
//        end();
//    }
//}
